#include "map_reader.h"
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <Eigen/Dense>
#include <memory>
#include <thread>
#include <chrono>
#include <numeric>
#include <boost/thread.hpp>

using namespace std::chrono_literals;

// Custom message includes
#include "agv_msgs/msg/odometry_lite.hpp"
#include "agv_msgs/msg/pose_with_confidence.hpp"
#include "agv_msgs/msg/order.hpp"
#include "agv_msgs/msg/map_node.hpp"
#include "agv_msgs/msg/agv_auto_status.hpp"
#include "agv_msgs/msg/agv_mode.hpp"
#include "agv_msgs/msg/nav_correct_flag.hpp"
#include "agv_msgs/msg/lidar_calib_percent.hpp"

// Custom service includes
#include "agv_srvs/srv/lidar_calib.hpp"
#include "agv_srvs/srv/local_order.hpp"
#include "agv_srvs/srv/set_operation_mode.hpp"
#include "agv_srvs/srv/set_laser_calib.hpp"
#include <shared_parameters/shared_params_interface.hpp>

struct PoseRecord {
	double x_;
	double y_;
	double theta_;
};

class AutoCalib : public rclcpp::Node {
	public:
		AutoCalib() : Node("laser_auto_calibration") {
			initializeParameters();

			initializeSubscribers();
			initializePublishers();
			initializeServices();
			initializeTimers();

			if(!initPointsData()) {
				RCLCPP_ERROR(this->get_logger(), "Failed to initialize map points data");
			}

			work_thread_ = std::make_shared<boost::thread>(&AutoCalib::work_cycle, this);
		}

		~AutoCalib() {
			work_thread_->interrupt();
			work_thread_->join();
		}

	private:
		inline double getdistance(geometry_msgs::msg::Pose cur, geometry_msgs::msg::Pose start){
	        return sqrt(pow((cur.position.x - start.position.x),2) + pow((cur.position.y - start.position.y),2));
	    }

		void initializeParameters() {
			// todo use shared_parameters
			if (!SharedParamsInterface::initialize()) {
	            RCLCPP_ERROR(get_logger(), "Failed to initialize shared memory interface");
	        } else {
		        try { 
		            auto params = SharedParamsInterface::getNodeParams("shared_parameters");
		            if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
		                RCLCPP_INFO(get_logger(), "Error getting shared parameters");
		            } else {
			            params = SharedParamsInterface::getNodeParams("lidar_guidance");
			            for (const auto& [name, value] : params) {
			                if (name.find("lidar1") == std::string::npos && 
			                    name.find("lidar2") == std::string::npos)
			                    continue;
			                
			                std::string param_name;
			                const std::string prefix = "lidar_info/";
			                size_t pos = name.find(prefix);

			                if (pos != std::string::npos) {
			                    size_t start_pos = pos + prefix.length();
			                    
			                    if (start_pos <= name.length()) {
			                        param_name = name.substr(start_pos); 
			                    } else {
			                        continue;
			                    }
			                } else {
			                    continue;
			                }
			                
			                RCLCPP_INFO_STREAM(get_logger(), "get param:" << param_name << ", value:" << value.toString());
			                switch(value.type) {
			                case SharedParamsInterface::ParamValue::Type::INT:
			                    this->declare_parameter(param_name, value.getValue<int>());
			                    break;
			                case SharedParamsInterface::ParamValue::Type::DOUBLE:
			                    this->declare_parameter(param_name, value.getValue<double>());
			                    break;
			                case SharedParamsInterface::ParamValue::Type::STRING:
			                    this->declare_parameter(param_name, value.getValue<std::string>());
			                    break;
			                case SharedParamsInterface::ParamValue::Type::BOOL:
			                    this->declare_parameter(param_name, value.getValue<bool>());
			                    break;
			                default:
			                    RCLCPP_WARN_STREAM(get_logger(), "Unsupported parameter type for " << name);
			                    break;
			                }
			            }
		        	}
		        } catch (const std::exception& e) {
		            RCLCPP_ERROR_STREAM(get_logger(), "Error reading parameters: " << e.what());
		        }
	    	}

			pose_x_ = this->get_parameter_or("lidar1/laser_pose_x", 0.0);
			pose_y_ = this->get_parameter_or("lidar1/laser_pose_y", 0.0);
			pose_theta_ = this->get_parameter_or("lidar1/laser_pose_theta", 0.0);
			isInversion_ = this->get_parameter_or("lidar1/isInversion", false);
			isInversion_2_ = this->get_parameter_or("lidar2/isInversion", false);
			lidar1_is_enable_ = this->get_parameter_or("lidar1/enable", true);
			lidar2_is_enable_ = this->get_parameter_or("lidar2/enable", false);
			vel_ = this->get_parameter_or("lidar_calibration/angel_vel", 0.1);
			y_bias_ = this->get_parameter_or("lidar_calibration/y_bias", 0.15);

			pose_x_old_ = pose_x_;
			pose_y_old_ = pose_y_;
			pose_theta_old_ = pose_theta_;

			isInversion_vec_.push_back(isInversion_);
			isInversion_vec_.push_back(isInversion_2_);
			isEnable_vec_.push_back(lidar1_is_enable_);
			isEnable_vec_.push_back(lidar2_is_enable_);

			start_pose_.position.x = 0;
			start_pose_.position.y = 0;
		}

		void initializeSubscribers() {
			pose_sub_ = this->create_subscription<agv_msgs::msg::PoseWithConfidence>(
					"/pose", 1, std::bind(&AutoCalib::pose_callback, this, std::placeholders::_1));

			landmark_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
					"/landmark", 1, std::bind(&AutoCalib::landmark_callback, this, std::placeholders::_1));

			move_info_sub_ = this->create_subscription<agv_msgs::msg::AgvAutoStatus>(
					"/auto_run_status", 1, std::bind(&AutoCalib::HandleMoveInfoMessage, this, std::placeholders::_1));

			mode_sub_ = this->create_subscription<agv_msgs::msg::AGVMode>(
					"/agvmode", 1, std::bind(&AutoCalib::HandleModeInfoMessage, this, std::placeholders::_1));

			odometry_sub_ = this->create_subscription<agv_msgs::msg::OdometryLite>(
					"/odom", 1, 
					std::bind(&AutoCalib::OdomSubscribeCallback, this, std::placeholders::_1));
		}

		void initializePublishers() {
			percent_pub_ = this->create_publisher<agv_msgs::msg::LidarCalibPercent>("/lidarCalib_percent", 1);
			ignore_flag_pub_ = this->create_publisher<agv_msgs::msg::NavCorrectFlag>("/set_nav_correct_flag", 1);
			vel_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>("/twist_mux/manual_vel", 10);
		}

		void initializeServices() {
			lidar_calib_service_ = this->create_service<agv_srvs::srv::LidarCalib>(
					"/lidarCalibration_service", 
					std::bind(&AutoCalib::lidar_calib_callback, this, 
						std::placeholders::_1, std::placeholders::_2));

			order_client_ = this->create_client<agv_srvs::srv::LocalOrder>("/local_order");
			laser_tf_client_ = this->create_client<agv_srvs::srv::SetLaserCalib>("/setLaserCalib");
			mode_client_ = this->create_client<agv_srvs::srv::SetOperationMode>("/operation_mode");
		}

		void initializeTimers() {
			init_map_timer_ = this->create_wall_timer(
					300ms, std::bind(&AutoCalib::InitMap, this));

			publish_percent_timer_ = this->create_wall_timer(
					1s, std::bind(&AutoCalib::PublishPercent, this));

			record_pose_timer_ = this->create_wall_timer(
					200ms, std::bind(&AutoCalib::RecordPose, this));

			publish_ignore_flag_timer_ = this->create_wall_timer(
					200ms, std::bind(&AutoCalib::PublishIgnoreFlag, this));
		}

		void pose_callback(const agv_msgs::msg::PoseWithConfidence::SharedPtr msg) {
			cur_pose_ = msg->pose.pose;
			tf2::Quaternion quat;
			tf2::fromMsg(cur_pose_.orientation, quat);
			tf2::Matrix3x3 mat(quat);
		    double roll, pitch, yaw;
		    mat.getRPY(roll, pitch, yaw);
		    cur_theta_ = yaw;
		}

		void landmark_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
			landmark_tag_ = msg->pose.position.z;
			landmark_pose_ = msg->pose;
			tf2::Quaternion quat;
			tf2::fromMsg(landmark_pose_.orientation, quat);
			tf2::Matrix3x3 mat(quat);
		    double roll, pitch, yaw;
		    mat.getRPY(roll, pitch, yaw);
		    landmark_theta_ = yaw;
		}

		void HandleMoveInfoMessage(const agv_msgs::msg::AgvAutoStatus::SharedPtr msg) {
			from_node_ = msg->fromnode;
			to_node_ = msg->tonode;
		}

		void HandleModeInfoMessage(const agv_msgs::msg::AGVMode::SharedPtr msg) {
			agv_mode_ = msg->mode;
		}

        void RecordPose() {
		    if(trigger_ == 4) {
		        if (from_node_ == node4_ && to_node_ == node4_ && record_trigger_ == false) {
		            if(abs(current_vel_.twist.angular.z)<0.0001 && abs(current_vel_.twist.linear.x) < 0.0001 && abs(current_vel_.twist.linear.y)<0.0001) {
		                PoseRecord pose;
		                pose.x_ = landmark_pose_.position.x;
		                pose.y_ = landmark_pose_.position.y;
		                pose.theta_ = landmark_theta_;
		                if(pose.theta_ < 0) {
		                    pose.theta_ = pose.theta_ + 2*M_PI;
		                }
		                pose_list_1_.push_back(pose);
		                record_trigger_ = true;
		                pose_x_sum_ = pose_x_sum_ + pose.x_;
		                pose_y_sum_ = pose_y_sum_ + pose.y_;
		                pose_theta_sum_ = pose_theta_sum_ + pose.theta_;
		                RCLCPP_ERROR_STREAM(this->get_logger(), "pose.x_ " << pose.x_ << " pose.y_ " << pose.y_ << " theta " << pose.theta_);
		            }
		        } else if (from_node_ == node5_ && to_node_ == node5_ && record_trigger_ == false) {
		            if(abs(current_vel_.twist.angular.z)<0.0001 && abs(current_vel_.twist.linear.x) < 0.0001 && abs(current_vel_.twist.linear.y)<0.0001) {
		                PoseRecord pose;
		                pose.x_ = landmark_pose_.position.x;
		                pose.y_ = landmark_pose_.position.y;
		                pose.theta_ = landmark_theta_;
		                if(pose.theta_ < 0) {
		                    pose.theta_ = pose.theta_ + 2*M_PI;
		                }
		                pose_list_2_.push_back(pose);
		                record_trigger_ = true;
		                pose_x_sum_ = pose_x_sum_ + pose.x_;
		                pose_y_sum_ = pose_y_sum_ + pose.y_;
		                pose_theta_sum_ = pose_theta_sum_ + pose.theta_;
		                RCLCPP_ERROR_STREAM(this->get_logger(), "pose.x_ " << pose.x_ << " pose.y_ " << pose.y_ << " theta " << pose.theta_);
		            }
		        } else if((from_node_ == node5_ && to_node_ != node5_) || (from_node_ == node4_ && to_node_ != node4_)) {
		            record_trigger_ = false;
		        }
		    } else {
		        pose_list_1_.clear();
		        pose_list_2_.clear();
		        pose_x_sum_ = 0;
		        pose_y_sum_ = 0;
		        pose_theta_sum_ = 0;
		        E_pose_x_1_ = 0;
		        E_pose_y_1_ = 0;
		        E_pose_theta_1_ = 0;

		        E_pose_x_2_ = 0;
		        E_pose_y_2_ = 0;
		        E_pose_theta_2_ = 0;

		        // Var_pose_x = 0;
		        // Var_pose_y = 0;
		        // Var_pose_theta = 0;
		        bias_x_max_1_ = 0;
		        bias_y_max_1_ = 0;
		        bias_theta_max_1_ = 0;

		        bias_x_max_2_ = 0;
		        bias_y_max_2_ = 0;
		        bias_theta_max_2_ = 0;
		    }
		}

		void PublishPercent() {
			auto per = agv_msgs::msg::LidarCalibPercent();

			if(calib_x_y_ == false && percent_ == 50) {
				per.percent = 100;
			} else {
				per.percent = percent_;
			}

			per.x = 65535;
			per.y = 65535;
			per.theta = 65535;

			if(calib_success_flag_ == false && percent_ == 50) {
				per.x = 65535;
				per.y = 65535;
				per.theta = pose_theta_;
			} else if(x_calib_success_flag_ == true && y_calib_success_flag_ == false && calib_success_flag_ == false) {
				per.x = pose_x_;
				per.y = 65535;
				per.theta = pose_theta_;
			} else if(x_calib_success_flag_ == false && y_calib_success_flag_ == true && calib_success_flag_ == false) {
				per.x = 65535;
				per.y = pose_y_;
				per.theta = pose_theta_;
			} else if(calib_success_flag_ == true) {
				per.x = pose_x_;
				per.y = pose_y_;
				per.theta = pose_theta_;
			}

			per.x2 = x2_;
			per.y2 = y2_;
			per.theta2 = theta2_;

			percent_pub_->publish(per);
		}

		void InitMap() {
			if(agv_mode_ == 3 && agv_mode_old_ == 0) {
				initPointsData();
			}
			agv_mode_old_ = agv_mode_;
		}

		void work_cycle() {
			rclcpp::Rate rate1(1.0);  // 1Hz
			rclcpp::Rate rate2(0.5);  // 0.5Hz

			while (rclcpp::ok()) {
				if (trigger_ == 2) {
					RCLCPP_WARN(this->get_logger(), "Calibration Interrupted!");
					rate1.sleep();
					continue;
				}

				if (yaw_calib_success_flag_ && !calib_x_y_) {
					RCLCPP_INFO(this->get_logger(), "Yaw Calibration finished!");
					rate1.sleep();
					continue;
				}

				if (yaw_calib_success_flag_ && x_y_calib_success_flag_) {
					RCLCPP_INFO(this->get_logger(), "Calibration finished!");
					rate1.sleep();
					continue;
				}

				if (percent_ < 0) {
					RCLCPP_INFO(this->get_logger(), "Calibration failed!");
					rate1.sleep();
					continue;
				}

				if (trigger_ == 1) {
					RCLCPP_WARN(this->get_logger(), "diff is %f", diff_);
					bool move_ret = false, cal_ret;

					if (agv_mode_ != 2) {
						RCLCPP_WARN(this->get_logger(), "agv mode is not semi-auto! please check!");
						rate1.sleep();
						continue;
					}

					if (diff_ < diff_threshold_) {
						move_ret = line_move(node0_, node1_);
						move_dir_ = 0;
					} else {
						RCLCPP_WARN(this->get_logger(), "please check map id and move agv to start node!");
						rate1.sleep();
						continue;
					}

					while (move_cycle_ >= 0 && trigger_ != 2) {
						if (move_cycle_ == 0) {
							if (percent_ < 45) {
								percent_ = -1;
								set_tf(pose_x_old_, pose_y_old_, pose_theta_old_, 1);
								calib_success_flag_ = false;
								RCLCPP_INFO(this->get_logger(), "Laser calibration failure! please check map set and chassis arguments!");
								break;
							} else if(percent_ == 50) {
								set_tf(pose_x_old_, pose_y_old_, pose_theta_old_, 1);
								calib_success_flag_ = false;
								RCLCPP_INFO(this->get_logger(), "Laser calibration x and y failure! but theta success! please record value!");
								break;
							} else if(percent_ == 75) {
								set_tf(pose_x_old_, pose_y_old_, pose_theta_old_, 1);
								calib_success_flag_ = false;
								RCLCPP_INFO(this->get_logger(), "Laser calibration x or y failure! but theta success! please record value!");
								break;
							}
						}

						RCLCPP_INFO(this->get_logger(), 
								"move_ret %d moveCycle is %d from node %d to node %d move dir %d node1_1_called %d node1_0_called %d yaw_calib_success_flag %d x_y_calib_success_flag %d calib_x_y %d",
								move_ret, move_cycle_, from_node_, to_node_, move_dir_, 
								node1_1_called_, node1_0_called_, yaw_calib_success_flag_, 
								x_y_calib_success_flag_, calib_x_y_);

						if (move_ret && from_node_ == node1_ && to_node_ == node1_ && 
								move_dir_ == 0 && !yaw_calib_success_flag_ && 
								trigger_ != 2 && !node1_0_called_){
							move_ret = line_move(node1_, node2_);
							node1_0_called_ = true;
						} else if (move_ret && from_node_ == node2_ && to_node_ == node2_ && 
								move_dir_ == 0 && !yaw_calib_success_flag_ && 
								trigger_ != 2 && !node2_0_called_) {
							rate2.sleep();
							node2_pose_record_1_ = cur_pose_;
							ignore_y_flag_ = true;
							move_ret = line_move(node2_, node3_);
							node2_0_called_ = true;
						} else if (move_ret && from_node_ == node3_ && to_node_ == node3_ && 
								move_dir_ == 0 && !yaw_calib_success_flag_ && 
								trigger_ != 2 && !node3_0_called_) {
							rate2.sleep();
							node3_pose_record_1_ = cur_pose_;
							move_ret = line_move(node3_, node4_);
							node3_0_called_ = true;
						} else if (move_ret && from_node_ == node4_ && to_node_ == node4_ && 
								move_dir_ == 0 && !yaw_calib_success_flag_ && 
								trigger_ != 2 && !node4_0_called_) {
							rate2.sleep();
							node4_pose_record_1_ = cur_pose_;
							ignore_y_flag_ = false;
							move_ret = line_move(node4_, node5_);
							node4_0_called_ = true;
						} else if (move_ret && from_node_ == node5_ && to_node_ == node5_ && 
								((move_dir_ == 0 && !node5_0_called_) || yaw_calib_success_flag_) && 
								trigger_ != 2) {
							rate2.sleep();
							if (!yaw_calib_success_flag_) {
								if (!yaw_correction_forced_) {
									bool ret = yaw_cal(move_dir_);
								}
								rate2.sleep();
								if (!yaw_calib_success_flag_) {
									move_dir_ = 1;
									node4_1_called_ = false;
									move_ret = line_move(node5_, node4_);
								}
							} else if (yaw_calib_success_flag_) {
								if(!x_y_calib_success_flag_ && calib_x_y_) {
									cal_ret = x_y_cal();
									if(cal_ret) {
										RCLCPP_INFO(this->get_logger(), "####1");
										break;
									} 
									continue;
								} else if(!calib_x_y_) {
									break;
								}
							}
							node5_0_called_ = true;
							node4_0_called_ = false;
							node3_0_called_ = false;
							node2_0_called_ = false;
							node1_1_called_ = false;
							yaw_correction_forced_ = false;
							force_correct_ = false;
						} else if (move_ret && from_node_ == node4_ && to_node_ == node4_ && 
								move_dir_ == 1 && !yaw_calib_success_flag_ && 
								trigger_ != 2 && !node4_1_called_) {
							rate2.sleep();
							node4_pose_record_2_ = cur_pose_;
							ignore_y_flag_ = true;
							move_ret = line_move(node4_, node3_);
							node4_1_called_ = true;
						} else if (move_ret && from_node_ == node3_ && to_node_ == node3_ && 
								move_dir_ == 1 && !yaw_calib_success_flag_ && 
								trigger_ != 2 && !node3_1_called_){
							rate2.sleep();
							node3_pose_record_2_ = cur_pose_;
							move_ret = line_move(node3_, node2_);
							node3_1_called_ = true;
						} else if (move_ret && from_node_ == node2_ && to_node_ == node2_ && 
								move_dir_ == 1 && !yaw_calib_success_flag_ && 
								trigger_ != 2 && !node2_1_called_) {
							rate2.sleep();
							node2_pose_record_2_ = cur_pose_;
							ignore_y_flag_ = false;
							move_ret = line_move(node2_, node1_);
							node2_1_called_ = true;
						} else if (move_ret && from_node_ == node1_ && to_node_ == node1_ && 
								((move_dir_ == 1 && !node1_1_called_) || yaw_calib_success_flag_) && 
								trigger_ != 2) {
							if(!yaw_calib_success_flag_) {
								if (!yaw_correction_forced_) {
									bool ret = yaw_cal(move_dir_);
									RCLCPP_INFO(this->get_logger(), "ret is %d", ret);
									rate2.sleep();
								}
								if (!yaw_calib_success_flag_) {
									move_dir_ = 0;
									node1_0_called_ = false;
								}
							} else if (yaw_calib_success_flag_) {
								if(!x_y_calib_success_flag_ && calib_x_y_) {
									cal_ret = x_y_cal();
									if(cal_ret) { 
										RCLCPP_INFO(this->get_logger(), "####2");
										break;
									}
								} else if(!calib_x_y_) {
									break;
								}
							}
							node1_1_called_ = true;
							node3_1_called_ = false;
							node2_1_called_ = false;
							node5_0_called_ = false;
							yaw_correction_forced_ = false;
							force_correct_ = false;
							RCLCPP_INFO(this->get_logger(), "moveCycle %d", move_cycle_);
							move_cycle_--;
						} else {
							if (map_stream_reader_->pointsMap[node2_].x == map_stream_reader_->pointsMap[node5_].x) { // Vertical
								if (move_dir_ == 0) {
									delta_x_ = cur_pose_.position.x - map_stream_reader_->pointsMap[node2_].x;
									if(std::abs(delta_x_) > y_bias_)
										yaw_correction_forced_ = true;
									delta_y_ = cur_pose_.position.y - node2_pose_record_1_.position.y;
								} else {
									delta_x_ = map_stream_reader_->pointsMap[node4_].x - cur_pose_.position.x;
									if(std::abs(delta_x_) > y_bias_) 
										yaw_correction_forced_ = true;
									delta_y_ = node4_pose_record_1_.position.y - cur_pose_.position.y;
								}
								delta_yaw_ = std::atan2(delta_x_, std::abs(delta_y_));
								if (std::abs(cur_theta_ - M_PI_2) < 10*M_PI/180) {
									delta_yaw_ = delta_yaw_;
								} else if (std::abs(cur_theta_ + M_PI_2) < 10*M_PI/180) {
									delta_yaw_ = -delta_yaw_;
								}
								RCLCPP_ERROR(this->get_logger(), "delta x %f delta y %f Vertical", delta_x_, delta_y_);
							} else if(map_stream_reader_->pointsMap[node2_].y == map_stream_reader_->pointsMap[node5_].y) { // Horizontal
								if (move_dir_ == 0) {
									delta_y_ = cur_pose_.position.y - map_stream_reader_->pointsMap[node2_].y;
									if(std::abs(delta_y_) > y_bias_) 
										yaw_correction_forced_ = true;
									delta_x_ = cur_pose_.position.x - node2_pose_record_1_.position.x;
								} else {
									delta_y_ = map_stream_reader_->pointsMap[node4_].y - cur_pose_.position.y;
									if(std::abs(delta_y_) > y_bias_) 
										yaw_correction_forced_ = true;
									delta_x_ = node4_pose_record_1_.position.x - cur_pose_.position.x;
								}
								delta_yaw_ = std::atan2(delta_y_, std::abs(delta_x_));
								if (std::abs(cur_theta_ - M_PI) <= 10*M_PI/180 || std::abs(cur_theta_ + M_PI) <= 10*M_PI/180) {
									delta_yaw_ = delta_yaw_;
								} else if(std::abs(cur_theta_ - 0) < 10*M_PI/180) {
									delta_yaw_ = -delta_yaw_;
								}
								RCLCPP_ERROR(this->get_logger(), "delta x %f delta y %f Horizontal", delta_x_, delta_y_);
							}

							RCLCPP_INFO(this->get_logger(), "yaw_correction_forced %d ignore_y_flag %d", 
									yaw_correction_forced_, ignore_y_flag_);

							if (!force_correct_ && yaw_correction_forced_) {
								pose_theta_ = pose_theta_ + delta_yaw_;
								RCLCPP_ERROR(this->get_logger(), "force set tf theta %f", pose_theta_);
								set_tf(pose_x_, pose_y_, pose_theta_, 1);
								force_correct_ = true;
							}
						}
						rate2.sleep();
					}
				}

				if (x_y_calib_success_flag_ && yaw_calib_success_flag_) {
					calib_success_flag_ = true;
					trigger_ = 0;
				} else {
					calib_success_flag_ = false;
				}
				rate2.sleep();
			}
		}

		bool Distance(int oprange) {
			rclcpp::Rate spin_rate(50);  // 50Hz rate
			int delay = 0, count = 0;
			geometry_msgs::msg::TwistStamped vel_msg;
			vel_msg.twist.linear.x = 0.0;

			while (trigger_ != 2 && rclcpp::ok()) {
				if (yaw_calib_success_flag_) {
					// Velocity control logic
					if (vel_msg.twist.linear.x < vel_) {
						vel_msg.twist.linear.x = vel_msg.twist.linear.x + 0.02;
					}
					vel_msg.twist.angular.z = 0.0;

					// Start position initialization
					if (start_pose_.position.y == 0.0 && start_pose_.position.x == 0.0) {
						start_pose_.position.y = current_odom_pose_.position.y;
						start_pose_.position.x = current_odom_pose_.position.x;
						RCLCPP_INFO(this->get_logger(), 
								"Start test - dis:%0.2f, cur pose:[%0.4f, %0.4f], vel:%.2f",
								oprange, current_odom_pose_.position.x, 
								current_odom_pose_.position.y, vel_);
						delay = 10;
					} 
					// Check if target distance reached
					else if (std::fabs(getdistance(start_pose_, current_odom_pose_)) >= oprange) {
						vel_msg.twist.linear.x = 0.0;
						vel_msg.twist.angular.z = 0.0;

						// Wait until robot stops
						if (std::fabs(current_vel_.twist.linear.x) < 0.0001) {
							if (delay-- <= 0) {
								RCLCPP_INFO(this->get_logger(),
										"End test - dis:[x:%0.4f, y:%0.4f]",
										std::fabs(current_odom_pose_.position.x - start_pose_.position.x),
										std::fabs(current_odom_pose_.position.y - start_pose_.position.y));

								// Reset start position
								start_pose_.position.x = 0.0;
								start_pose_.position.y = 0.0;
								delay = 0;
								break;
							}
						}
					} 
					// Moving status update
					else {
						RCLCPP_INFO(this->get_logger(),
								"moving - target:%0.2f, dis:[%0.4f], vel:%.2f",
								oprange, std::fabs(getdistance(start_pose_, current_odom_pose_)),
								vel_msg.twist.linear.x);
					}
				}

				// Publish velocity command
				vel_pub_->publish(vel_msg);
				spin_rate.sleep();
			}
			return true;
		}

		bool Angular(int oprange) {
			rclcpp::Rate spin_rate(50);  // 50Hz rate
			int delay = 0, count = 0;
			double theta = 0;
			bool init_yaw = false;
			if(trigger_ == 2) return false;
			RCLCPP_INFO(this->get_logger(), "##Angular :%d", oprange);
			while (trigger_ != 2 && rclcpp::ok()) {
				geometry_msgs::msg::TwistStamped vel_msg;

				if (yaw_calib_success_flag_) {
					vel_msg.twist.angular.z = vel_;
					vel_msg.twist.linear.x = 0;

					// Convert quaternion to RPY using tf2
					tf2::Quaternion quat;
					tf2::fromMsg(current_odom_pose_.orientation, quat);

					double roll, pitch, yaw;
					tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

					if (!init_yaw) {
						init_yaw = true;
						count = 0;
						theta = yaw + (vel_ > 0 ? oprange : oprange * -1) * M_PI / 180.0;

						if(theta > M_PI) theta = theta - 2 * M_PI;
						if(theta <= -M_PI) theta = theta + 2 * M_PI;

						RCLCPP_DEBUG(this->get_logger(), 
								"Start test - current ang: %.2f°, target ang: %.2f°, ang vel: %.2f", 
								yaw * 180.0 / M_PI, 
								theta * 180.0 / M_PI, 
								vel_);
					} else {
						if (count > 100 && std::fabs(yaw - theta) <= 0.02) {
							vel_msg.twist.linear.x = 0;
							vel_msg.twist.angular.z = 0;

							if (std::fabs(current_vel_.twist.angular.z) < 0.0001) {
								if (delay-- <= 0) {
									RCLCPP_DEBUG(this->get_logger(),
											"End test - stop ang: %.2f°, target ang: %.2f°, diff: %.2f°",
											yaw * 180.0 / M_PI,
											theta * 180.0 / M_PI,
											std::fabs(yaw - theta) * 180.0 / M_PI);

									theta = 0;
									delay = 0;
									init_yaw = false;
									break;
								}
							}
						} else {
							count++;
							RCLCPP_DEBUG(this->get_logger(),
									"Rotating - ang: %.2f°, diff: %.2f°, ang vel: %.2f°",
									yaw * 180.0 / M_PI,
									std::fabs(yaw - theta) * 180.0 / M_PI,
									current_vel_.twist.angular.z * 180.0 / M_PI);
						}
					}
				}

				vel_pub_->publish(vel_msg);
				spin_rate.sleep();
			}
			return true;
		}

		bool lidar_calib_callback(
		    const std::shared_ptr<agv_srvs::srv::LidarCalib::Request> req,
		    std::shared_ptr<agv_srvs::srv::LidarCalib::Response> resp) {
		    
		    trigger_ = req->trigger;
		    node0_ = req->node0;
		    node1_ = req->node1;
		    node2_ = req->node2;
		    node3_ = req->node3;
		    node4_ = req->node4;
		    node5_ = req->node5;
		    calib_x_y_ = !req->calib_x_y;

		    // Set node0 pose from map data
		    node0_pose_.position.x = map_stream_reader_->pointsMap[node0_].x;
		    node0_pose_.position.y = map_stream_reader_->pointsMap[node0_].y;
		    diff_ = getdistance(node0_pose_, cur_pose_);

		    RCLCPP_INFO(this->get_logger(), "call server-----------------trigger: %d", trigger_);

		    if(trigger_ == 1) {
		        if(diff_ > diff_threshold_) {
		            lidar_msg_ = "please move agv to node 0";
		            resp->msg = lidar_msg_;
		            resp->success = 2;
		        } else if(agv_mode_ != 2) {
		            lidar_msg_ = "agv mode is not semi-auto, please check!";
		            resp->msg = lidar_msg_;
		            resp->success = 2;
		        } else if(move_cycle_ == 0) {
		            lidar_msg_ = "calibration failed, please record theta and x y, then cancel calibration or restart";
		            resp->msg = lidar_msg_;
		            resp->success = 2;
		        } else if(percent_ > 0 && percent_ < 100 && calib_x_y_) {
		            lidar_msg_ = "calibrating";
		            resp->msg = lidar_msg_;
		            resp->success = 0;
		        } else if(percent_ > 0 && percent_ < 50 && !calib_x_y_) {
		            lidar_msg_ = "calibrating";
		            resp->msg = lidar_msg_;
		            resp->success = 0;
		        } else {
		            auto mode_request = std::make_shared<agv_srvs::srv::SetOperationMode::Request>();
		            mode_request->mode = "auto";
		            mode_request->sender = 1;
		            mode_request->submode = "local";

		            auto response_ptr = std::make_shared<std::shared_ptr<agv_srvs::srv::LidarCalib::Response>>(resp);

		            mode_client_->async_send_request(mode_request,
		                [this, response_ptr](rclcpp::Client<agv_srvs::srv::SetOperationMode>::SharedFuture future) {
		                    try {
		                        auto mode_response = future.get();
		                        if (mode_response->success) {
		                            RCLCPP_INFO(this->get_logger(), "Mode switched to AUTO successfully");
		                            
		                            (*response_ptr)->msg = "start";
		                            (*response_ptr)->success = 1;

		                            lidar_msg_ = "start";
		                            percent_ = 25;
		                            move_cycle_ = 20;

		                            // Reset all calibration flags
		                            calib_success_flag_ = false;
		                            yaw_calib_success_flag_ = false;
		                            x_y_calib_success_flag_ = false;
		                            y_calib_success_flag_ = false;
		                            x_calib_success_flag_ = false;
		                            cycle_count_ = 0;

		                            // Clear temporary data
		                            fixed_x_.clear();
		                            fixed_y_.clear();
		                            delta_yaw_list_.clear();

		                            // Reset node called flags
		                            node1_0_called_ = false;
		                            node2_0_called_ = false;
		                            node3_0_called_ = false;
		                            node4_0_called_ = false;
		                            node5_0_called_ = false;
		                            node5_1_called_ = false;
		                            node4_1_called_ = false;
		                            node3_1_called_ = false;
		                            node2_1_called_ = false;
		                            node1_1_called_ = false;

		                            // Handle secondary lidar if enabled
		                            if(lidar2_is_enable_) {
		                                try {
		                                    geometry_msgs::msg::TransformStamped tf_front = tf_buffer_->lookupTransform(
		                                            "base_link", "laser_link_1", 
		                                            tf2::TimePointZero,
		                                            tf2::durationFromSec(5.0));

		                                    geometry_msgs::msg::TransformStamped tf_back = tf_buffer_->lookupTransform(
		                                            "base_link", "laser_link_2",
		                                            tf2::TimePointZero,
		                                            tf2::durationFromSec(5.0));

		                                    tf2::Quaternion front_quat, back_quat;
		                                    tf2::fromMsg(tf_front.transform.rotation, front_quat);
		                                    tf2::fromMsg(tf_back.transform.rotation, back_quat);

		                                    double roll1, pitch1, yaw1, roll2, pitch2, yaw2;
		                                    tf2::Matrix3x3(front_quat).getRPY(roll1, pitch1, yaw1);
		                                    tf2::Matrix3x3(back_quat).getRPY(roll2, pitch2, yaw2);

		                                    // Set point centers
		                                    point_center_[0] = tf_front.transform.translation.x;
		                                    point_center_[1] = tf_front.transform.translation.y;
		                                    point_center_[2] = 0;

		                                    point_rotate_[0] = tf_back.transform.translation.x;
		                                    point_rotate_[1] = tf_back.transform.translation.y;
		                                    point_rotate_[2] = 0;

		                                    pose_theta_old_ = yaw2;
		                                    deltaX_ = std::abs(tf_back.transform.translation.x - tf_front.transform.translation.x);
		                                    deltaY_ = std::abs(tf_back.transform.translation.y - tf_front.transform.translation.y);
		                                    deltaT_ = yaw2 - yaw1;

		                                } catch (const tf2::TransformException &ex) {
		                                    RCLCPP_ERROR(this->get_logger(), "TF lookup failed: %s", ex.what());
		                                    percent_ = -2;
		                                    (*response_ptr)->msg = "TF lookup failed for secondary lidar";
		                                    (*response_ptr)->success = 2;
		                                }
		                            }
		                        } else {
		                            lidar_msg_ = "Mode switch failed: " + mode_response->message;
		                            (*response_ptr)->msg = lidar_msg_;
		                            (*response_ptr)->success = 2;
		                            RCLCPP_ERROR(this->get_logger(), "Mode switch failed: %s", mode_response->message.c_str());
		                        }
		                    } catch (const std::exception& e) {
		                        lidar_msg_ = "Mode switch exception: " + std::string(e.what());
		                        (*response_ptr)->msg = lidar_msg_;
		                        (*response_ptr)->success = 2;
		                        RCLCPP_ERROR(this->get_logger(), "Mode switch exception: %s", e.what());
		                    }
		                });

		            lidar_msg_ = "mode switch in progress";
		            resp->msg = lidar_msg_;
		            resp->success = 0;
		        }
		    } else if(trigger_ == 2) {
		        // Cancel calibration
		        lidar_msg_ = "cancel";
		        percent_ = 0;
		        resp->msg = lidar_msg_;
		        resp->success = 1;

		        // Reset all flags and data
		        calib_success_flag_ = false;
		        yaw_calib_success_flag_ = false;
		        x_y_calib_success_flag_ = false;
		        x_calib_success_flag_ = false;
		        y_calib_success_flag_ = false;
		        cycle_count_ = 0;

		        // Restore original transform
		        set_tf(pose_x_old_, pose_y_old_, pose_theta_old_, 1);

		        move_dir_ = 0;

		        // Reset node called flags
		        node1_0_called_ = false;
		        node2_0_called_ = false;
		        node3_0_called_ = false;
		        node4_0_called_ = false;
		        node5_0_called_ = false;
		        node5_1_called_ = false;
		        node4_1_called_ = false;
		        node3_1_called_ = false;
		        node2_1_called_ = false;
		        node1_1_called_ = false;
		    } else if(trigger_ == 3) {
		        // Status check
		        if(!calib_success_flag_ && percent_ == 25) {
		            lidar_msg_ = "calibrating ...";
		            resp->msg = lidar_msg_;
		            resp->success = 0;
		        } else if(calib_success_flag_) {
		            lidar_msg_ = "calibration finish!";
		            resp->x = pose_x_;
		            resp->y = pose_y_;
		            resp->angle = pose_theta_;
		            resp->msg = lidar_msg_;
		            resp->success = 1;
		        } else if(!calib_success_flag_ && percent_ == 0) {
		            lidar_msg_ = "calibration not start!";
		            resp->msg = lidar_msg_;
		            resp->success = 2;
		        } else if(!calib_success_flag_ && percent_ == 50) {
		            lidar_msg_ = "theta calibration success! but x y not yet!";
		            resp->x = 65535;
		            resp->y = 65535;
		            resp->angle = pose_theta_;
		            resp->msg = lidar_msg_;
		            resp->success = 2;
		        } else if(x_calib_success_flag_ && !y_calib_success_flag_ && !calib_success_flag_) {
		            lidar_msg_ = "theta and x calibration success! but y not yet!";
		            resp->x = pose_x_;
		            resp->y = 65535;
		            resp->angle = pose_theta_;
		            resp->msg = lidar_msg_;
		            resp->success = 2;
		        } else if(!x_calib_success_flag_ && y_calib_success_flag_ && !calib_success_flag_) {
		            lidar_msg_ = "theta and y calibration success! but x not yet!";
		            resp->x = 65535;
		            resp->y = pose_y_;
		            resp->angle = pose_theta_;
		            resp->msg = lidar_msg_;
		            resp->success = 2;
		        } else if(percent_ < 0) {
		            lidar_msg_ = "calibration failed";
		            resp->msg = lidar_msg_;
		            resp->success = 2;
		        }
		    } else if(trigger_ == 4) {
		        // Handle trigger 4 case if needed
		        resp->msg = "Trigger 4 not implemented";
		        resp->success = 2;
		    } else {
		        resp->msg = "request error";
		        resp->success = 2;
		    }

		    return true;
		}

		bool x_y_cal() {
			RCLCPP_WARN(this->get_logger(), "start x y caculating!");
			bool ret = false;
			vel_ = 0.08;

			// Record initial poses
			scan_pose_array_[0] = cur_pose_;
			odom_pose_array_[0] = current_odom_pose_;

			if(trigger_ == 2) return false;

			// First rotation sequence (90 degrees)
			Angular(90);
			std::this_thread::sleep_for(std::chrono::seconds(3));
			scan_pose_array_[1] = cur_pose_;
			odom_pose_array_[1] = current_odom_pose_;

			if(trigger_ == 2) return false;
			Angular(90);
			std::this_thread::sleep_for(std::chrono::seconds(3));
			scan_pose_array_[2] = cur_pose_;
			odom_pose_array_[2] = current_odom_pose_;

			if(trigger_ == 2) return false;
			Angular(90);
			std::this_thread::sleep_for(std::chrono::seconds(3));
			scan_pose_array_[3] = cur_pose_;
			odom_pose_array_[3] = current_odom_pose_;

			if(trigger_ == 2) return false;
			Angular(90);

			// Check calibration results
			ret = record_x_y();
			if(ret == true){
				return ret;
			}

			std::this_thread::sleep_for(std::chrono::seconds(3));

			// Reverse direction
			vel_ = -0.08;
			scan_pose_array_[0] = cur_pose_;
			odom_pose_array_[0] = current_odom_pose_;

			if(trigger_ == 2) return false;
			Angular(90);
			std::this_thread::sleep_for(std::chrono::seconds(3));
			scan_pose_array_[1] = cur_pose_;
			odom_pose_array_[1] = current_odom_pose_;

			if(trigger_ == 2) return false;
			Angular(90);
			std::this_thread::sleep_for(std::chrono::seconds(3));
			scan_pose_array_[2] = cur_pose_;
			odom_pose_array_[2] = current_odom_pose_;

			if(trigger_ == 2) return false;
			Angular(90);
			std::this_thread::sleep_for(std::chrono::seconds(3));
			scan_pose_array_[3] = cur_pose_;
			odom_pose_array_[3] = current_odom_pose_;

			// Check calibration results again
			ret = record_x_y();
			if(ret == true){
				return ret;
			}

			// Calculate average offsets
			pose_x_ = pose_x_ + (std::accumulate(fixed_x_.begin(), fixed_x_.end(), 0.)/fixed_x_.size())/2;
			pose_y_ = pose_y_ + (std::accumulate(fixed_y_.begin(), fixed_y_.end(), 0.)/fixed_y_.size())/2;

			if(trigger_ == 2) return false;
			Angular(90);
			std::this_thread::sleep_for(std::chrono::seconds(3));

			RCLCPP_ERROR(this->get_logger(), "pose x %.3f pose y %.3f pose_theta_ %.3f for x y cal!", 
					pose_x_, pose_y_, pose_theta_);

			// Update transform
			set_tf(pose_x_, pose_y_, pose_theta_, 1);

			// Clear temporary data
			fixed_x_.clear();
			fixed_y_.clear();
			std::this_thread::sleep_for(std::chrono::seconds(3));

			return false;
		}

		bool yaw_cal(bool move_dir) {
			tf2::Quaternion quat;
			tf2::fromMsg(cur_pose_.orientation, quat);
			tf2::Matrix3x3 mat(quat);
		    double roll, pitch, yaw;
		    mat.getRPY(roll, pitch, yaw);
		    double cur_theta = yaw;
		
			if(map_stream_reader_->pointsMap[node2_].x == map_stream_reader_->pointsMap[node5_].x) { // Vertical case
				if(move_dir == 0) {
					delta_x_ = node4_pose_record_1_.position.x - map_stream_reader_->pointsMap[node4_].x;
				} else {
					delta_x_ = map_stream_reader_->pointsMap[node2_].x - node2_pose_record_2_.position.x;
				}
				delta_y_ = node4_pose_record_1_.position.y - node2_pose_record_1_.position.y;

				RCLCPP_ERROR(this->get_logger(), "delta x %f delta y %f", delta_x_, delta_y_);

				delta_yaw_ = std::atan2(delta_x_, std::abs(delta_y_));

				// Adjust yaw based on current orientation
				if(std::abs(cur_theta - M_PI_2) < 10*M_PI/180) {
					delta_yaw_ = delta_yaw_;
				}
				else if(std::abs(cur_theta + M_PI_2) < 10*M_PI/180) {
					delta_yaw_ = -delta_yaw_;
				}
			} else if(map_stream_reader_->pointsMap[node2_].y == map_stream_reader_->pointsMap[node5_].y) { // Horizontal case
				delta_x_ = node4_pose_record_1_.position.x - node2_pose_record_1_.position.x;

				if(move_dir == 0) {
					delta_y_ = node4_pose_record_1_.position.y - map_stream_reader_->pointsMap[node4_].y;
				} else {
					delta_y_ = map_stream_reader_->pointsMap[node2_].y - node2_pose_record_1_.position.y;
				}

				delta_yaw_ = std::atan2(delta_y_, std::abs(delta_x_));
				RCLCPP_ERROR(this->get_logger(), "delta x %f delta y %f", delta_x_, delta_y_);

				// Adjust yaw based on current orientation
				if(std::abs(cur_theta - M_PI) <= 10*M_PI/180 || std::abs(cur_theta + M_PI) <= 10*M_PI/180) {
					delta_yaw_ = delta_yaw_;
				}
				else if(std::abs(cur_theta - 0) < 10*M_PI/180) {
					delta_yaw_ = -delta_yaw_;
				}
			}

			RCLCPP_ERROR(this->get_logger(), "delta_yaw is %f", delta_yaw_);

			// Update pose theta based on inversion flag
			if(!isInversion_) {
				pose_theta_ = pose_theta_ + delta_yaw_;
			} else {
				pose_theta_ = pose_theta_ + delta_yaw_;
			}

			// Store yaw values and calculate average
			delta_yaw_list_.push_back(delta_yaw_);
			delta_yaw_avg_ = std::accumulate(delta_yaw_list_.begin(), delta_yaw_list_.end(), 0.) / delta_yaw_list_.size();

			RCLCPP_ERROR(this->get_logger(), 
					"pose x %.3f pose y %.3f pose_theta %.3f cycleCount %d cycleCount%%3 %d abs yaw avg %.3f M_PI/a_t %.3f lidar2_isEnable %d",
					pose_x_, pose_y_, pose_theta_, cycle_count_, cycle_count_%3, 
					std::abs(delta_yaw_avg_), M_PI/a_t_, lidar2_is_enable_);

			cycle_count_++;

			if(std::abs(delta_yaw_avg_) < a_t_) { // Success condition (0.5° threshold)
				yaw_calib_success_flag_ = true;
				percent_ = 50;
				move_dir = 1;
				return true;
			} else if(std::abs(delta_yaw_avg_) >= a_t_) {
				RCLCPP_WARN(this->get_logger(), 
						"set tf at count %d pose x %.3f pose y %.3f pose_theta %.3f",
						cycle_count_, pose_x_, pose_y_, pose_theta_);

				set_tf(pose_x_, pose_y_, pose_theta_, 1);

				if(lidar2_is_enable_) {
					bool ret = SetBackLaserTF(pose_theta_ - pose_theta_old_);
				}

				delta_yaw_list_.clear();
				pose_theta_old_ = pose_theta_;
			}

			cycle_count_++;
			return true;
		}

		bool SetBackLaserTF(double yaw_diff) {
			double roll1, pitch1, yaw1, roll2, pitch2, yaw2;

			try {
				// Wait for and get front laser transform
				geometry_msgs::msg::TransformStamped tf_front = tf_buffer_->lookupTransform(
						"base_link", "laser_link_1", 
						tf2::TimePointZero,
						tf2::durationFromSec(5.0)
						);

				// Wait for and get back laser transform
				geometry_msgs::msg::TransformStamped tf_back = tf_buffer_->lookupTransform(
						"base_link", "laser_link_2",
						tf2::TimePointZero,
						tf2::durationFromSec(5.0)
						);

				// Convert quaternions to RPY for back laser
				tf2::Quaternion back_quat;
				tf2::fromMsg(tf_back.transform.rotation, back_quat);
				tf2::Matrix3x3(back_quat).getRPY(roll1, pitch1, yaw1);

				RCLCPP_ERROR(this->get_logger(), 
						"back to base x %.3f y %.3f z %.3f yaw %.3f", 
						tf_back.transform.translation.x,
						tf_back.transform.translation.y,
						tf_back.transform.translation.z,
						yaw1);

				// Convert quaternions to RPY for front laser
				tf2::Quaternion front_quat;
				tf2::fromMsg(tf_front.transform.rotation, front_quat);
				tf2::Matrix3x3(front_quat).getRPY(roll2, pitch2, yaw2);

				RCLCPP_ERROR(this->get_logger(),
						"front to base x %.3f y %.3f z %.3f yaw %.3f",
						tf_front.transform.translation.x,
						tf_front.transform.translation.y,
						tf_front.transform.translation.z,
						yaw2);

				// Set point centers
				point_center_[0] = tf_front.transform.translation.x;
				point_center_[1] = tf_front.transform.translation.y;
				point_center_[2] = 0;

				point_rotate_[0] = tf_back.transform.translation.x;
				point_rotate_[1] = tf_back.transform.translation.y;
				point_rotate_[2] = 0;
			} catch (const tf2::TransformException &ex) {
				RCLCPP_ERROR(this->get_logger(), 
						"TF lookup failed: %s", ex.what());
				percent_ = -2;
				return false;
			} catch (...) {
				RCLCPP_ERROR(this->get_logger(),
						"Unknown error occurred during TF lookup");
				percent_ = -2;
				return false;
			}

			// Calculate rotation
			Eigen::Matrix3d rotationMatrix = Eigen::AngleAxisd(
					yaw_diff, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();
			Eigen::Vector3d rotatedPoint = rotationMatrix * 
				(point_rotate_ - point_center_) + point_center_;

			x2_ = rotatedPoint[0];
			y2_ = rotatedPoint[1];
			theta2_ = yaw1 + yaw_diff;

			double theta_ = pose_theta_ - deltaT_;
			RCLCPP_ERROR(this->get_logger(),
					"back tf set x2 %.3f y2 %.3f theta2 %.3f theta_ %.3f",
					x2_, y2_, theta2_, theta_);

			// Clear and populate vectors for TF setting
			x_vec_.clear();
			y_vec_.clear();
			theta_vec_.clear();

			x_vec_.push_back(pose_x_);
			x_vec_.push_back(x2_);
			y_vec_.push_back(pose_y_);
			y_vec_.push_back(y2_);
			theta_vec_.push_back(pose_theta_);
			theta_vec_.push_back(theta2_);

			set_tf_2(x_vec_, y_vec_, theta_vec_, 2);
			return true;
		}

		bool set_tf_2(std::vector<float> x_vec, std::vector<float> y_vec, 
				std::vector<float> theta_vec, int num) {
			auto request = std::make_shared<agv_srvs::srv::SetLaserCalib::Request>();
			request->laser_num = num;

			// Populate enable flags
			request->enable.push_back(isEnable_vec_[0]);
			request->enable.push_back(isEnable_vec_[1]);

			// Populate inversion flags
			request->isinversion.push_back(isInversion_vec_[0]);
			request->isinversion.push_back(isInversion_vec_[1]);

			// Set position vectors
			request->x = x_vec;
			request->y = y_vec;

			// Normalize angles to [-π, π]
			for (auto& theta : theta_vec) {
				if(theta > M_PI) { theta = theta - 2 * M_PI;}
    			if(theta <= -M_PI) { theta = theta + 2 * M_PI;}
			}
			request->yaw = theta_vec;

			// Wait for service to be available
			if (!laser_tf_client_->wait_for_service(std::chrono::seconds(1))) {
				RCLCPP_ERROR(this->get_logger(), "Service not available after waiting");
				return false;
			}

			// Send request asynchronously
			auto future = laser_tf_client_->async_send_request(request);
		    auto status = future.wait_for(std::chrono::seconds(5)); // 设置等待超时时间

		    if (status == std::future_status::ready) {
		        auto response = future.get();
		        RCLCPP_INFO(this->get_logger(), "Service response: %s", 
		                response->success ? "Success" : "Failure");
		        return response->success;
		    } else if (status == std::future_status::timeout) {
		        RCLCPP_ERROR(this->get_logger(), "Service call timed out");
		        return false;
		    } else {
		        RCLCPP_ERROR(this->get_logger(), "Failed to get service response due to an unknown error.");
		        return false;
		    }
		}

		bool line_move(int fromNode, int toNode) {
	        RCLCPP_INFO(this->get_logger(), "line_move-----------------distance");

	        if (!order_client_->wait_for_service(std::chrono::seconds(10))) {
	            RCLCPP_ERROR(this->get_logger(), "v_local_order service not available after waiting 10 seconds");
	            return false;
	        }

	        auto order = std::make_shared<agv_msgs::msg::Order>();
	        order->orderid = std::to_string(order_num_);

	        auto node = std::make_shared<agv_msgs::msg::MapNode>();
	        auto request = std::make_shared<agv_srvs::srv::LocalOrder::Request>();

	        node->nodeid = std::to_string(fromNode);
	        order->nodes.push_back(*node);
	        node->nodeid = std::to_string(toNode);
	        order->nodes.push_back(*node);

	        request->order = *order;
	        request->recycle = false;

	        auto future = order_client_->async_send_request(request);
	        
	        std::future_status status = future.wait_for(std::chrono::seconds(10));
	        
	        if (status != std::future_status::ready) {
	            RCLCPP_ERROR(this->get_logger(), "v_local_order service call timed out after 30 seconds");
	            return false;
	        }

	        auto response = future.get();
	        RCLCPP_WARN(this->get_logger(), "%s", response->message.c_str());

	        order_num_++;
	        return response->success;
	    }

		bool set_tf(double x, double y, double theta, int laser_num) {
	        if (!laser_tf_client_->wait_for_service(std::chrono::seconds(10))) {
	            RCLCPP_ERROR(this->get_logger(), "set_laser_calib service not available after waiting 10 seconds");
	            return false;
	        }

	        auto request = std::make_shared<agv_srvs::srv::SetLaserCalib::Request>();
	        request->laser_num = laser_num;

	        // Populate enable flags
	        request->enable.push_back(lidar1_is_enable_);
	        request->isinversion.push_back(isInversion_);

	        // Set position
	        request->x.push_back(x);
	        request->y.push_back(y);

	        // Normalize angle to [-π, π]
	        if(theta > M_PI) { theta = theta - 2 * M_PI;}
	        if(theta <= -M_PI) { theta = theta + 2 * M_PI;}
	        request->yaw.push_back(theta);

	        auto future = laser_tf_client_->async_send_request(request);
	        
	        std::future_status status = future.wait_for(std::chrono::seconds(10));
	        
	        if (status != std::future_status::ready) {
	            RCLCPP_ERROR(this->get_logger(), "set_laser_calib service call timed out after 30 seconds");
	            return false;
	        }

	        auto response = future.get();
	        RCLCPP_INFO(this->get_logger(), "Service response: %s", 
	                response->success ? "Success" : "Failure");

	        return response->success;
	    }

		bool record_x_y() {
			RCLCPP_ERROR(this->get_logger(), "start recording.");
			bool x_y_calib_success_flag_1 = false;
			bool x_calib_success_flag_1 = false;
			bool y_calib_success_flag_1 = false;
			double fixed_delta_y, fixed_delta_x;

			// Convert quaternion to yaw using tf2
			tf2::Quaternion scan_quat;
			tf2::fromMsg(scan_pose_array_[0].orientation, scan_quat);
			tf2::Matrix3x3 mat(scan_quat);
		    double roll, pitch, yaw;
		    mat.getRPY(roll, pitch, yaw);
		    double scan_yaw = yaw;
			
			if(std::abs(scan_yaw) <= 30*M_PI/180) {
				// Calculate position differences for first case
				fixed_delta_x = scan_pose_array_[0].position.x - scan_pose_array_[2].position.x - 
					(odom_pose_array_[0].position.x - odom_pose_array_[2].position.x);
				fixed_delta_y = scan_pose_array_[0].position.y - scan_pose_array_[2].position.y - 
					(odom_pose_array_[0].position.y - odom_pose_array_[2].position.y);

				fixed_x_.push_back(fixed_delta_x);
				fixed_y_.push_back(fixed_delta_y);
				RCLCPP_WARN(this->get_logger(), "fixed_delta_x %f fixed_delta_y %f", fixed_delta_x, fixed_delta_y);

				// Check thresholds for first case
				if(std::abs(fixed_delta_x) < delta_threshold_) {
					RCLCPP_INFO(this->get_logger(), "x_calib_success_flag_1 true");
					x_calib_success_flag_1 = true;
				}
				if(std::abs(fixed_delta_y) < delta_threshold_) {
					RCLCPP_INFO(this->get_logger(), "y_calib_success_flag_1 true");
					y_calib_success_flag_1 = true;
				}

				// Calculate position differences based on velocity direction
				if (vel_ > 0) {
					fixed_delta_y = scan_pose_array_[1].position.x - scan_pose_array_[3].position.x - 
						(odom_pose_array_[1].position.x - odom_pose_array_[3].position.x);
					fixed_delta_x = scan_pose_array_[3].position.y - scan_pose_array_[1].position.y - 
						(odom_pose_array_[3].position.y - odom_pose_array_[1].position.y);
				} else {
					fixed_delta_y = scan_pose_array_[3].position.x - scan_pose_array_[1].position.x - 
						(odom_pose_array_[3].position.x - odom_pose_array_[1].position.x);
					fixed_delta_x = scan_pose_array_[1].position.y - scan_pose_array_[3].position.y - 
						(odom_pose_array_[1].position.y - odom_pose_array_[3].position.y);
				}

				fixed_x_.push_back(fixed_delta_x);
				fixed_y_.push_back(fixed_delta_y);
				RCLCPP_WARN(this->get_logger(), "fixed_delta_x %f fixed_delta_y %f", fixed_delta_x, fixed_delta_y);

				// Update calibration flags based on results
				if(std::abs(fixed_delta_x) < delta_threshold_ && x_calib_success_flag_1) {
					x_calib_success_flag_ = true;
					RCLCPP_INFO(this->get_logger(), "x_calib_success_flag true");
				}
				if(std::abs(fixed_delta_y) < delta_threshold_ && y_calib_success_flag_1) {
					y_calib_success_flag_ = true;
					RCLCPP_INFO(this->get_logger(), "y_calib_success_flag_ true");
				}

				// Determine calibration status and set percentage
				if (x_calib_success_flag_ && !y_calib_success_flag_) {
					percent_ = 75;
					RCLCPP_INFO(this->get_logger(), "x calibration success !");
				} else if(!x_calib_success_flag_ && y_calib_success_flag_) {
					percent_ = 75;
					RCLCPP_INFO(this->get_logger(), "y calibration success !");
				} else if(x_calib_success_flag_ && y_calib_success_flag_) {
					x_y_calib_success_flag_ = true;
					RCLCPP_INFO(this->get_logger(), "x and y calibration success !");
					percent_ = 100;
					return true;
				}
			} else if (std::abs(scan_yaw - M_PI) <= 30*M_PI/180 || std::abs(scan_yaw + M_PI) <= 30*M_PI/180) {
				// Second case: angle near PI or -PI
				fixed_delta_x = scan_pose_array_[2].position.x - scan_pose_array_[0].position.x - 
					(odom_pose_array_[2].position.x - odom_pose_array_[0].position.x);
				fixed_delta_y = scan_pose_array_[2].position.y - scan_pose_array_[0].position.y - 
					(odom_pose_array_[2].position.y - odom_pose_array_[0].position.y);

				fixed_x_.push_back(fixed_delta_x);
				fixed_y_.push_back(fixed_delta_y);
				RCLCPP_WARN(this->get_logger(), "fixed_delta_x %f fixed_delta_y %f", fixed_delta_x, fixed_delta_y);

				// Check thresholds for second case
				if(std::abs(fixed_delta_x) < delta_threshold_) {
					x_calib_success_flag_1 = true;
				}
				if(std::abs(fixed_delta_y) < delta_threshold_) {
					y_calib_success_flag_1 = true;
				}

				// Calculate position differences based on velocity direction
				if(vel_ > 0) {
					fixed_delta_x = scan_pose_array_[3].position.y - scan_pose_array_[1].position.y - 
						(odom_pose_array_[3].position.y - odom_pose_array_[1].position.y);
					fixed_delta_y = scan_pose_array_[1].position.x - scan_pose_array_[3].position.x - 
						(odom_pose_array_[1].position.x - odom_pose_array_[3].position.x);
				} else {
					fixed_delta_x = scan_pose_array_[1].position.y - scan_pose_array_[3].position.y - 
						(odom_pose_array_[1].position.y - odom_pose_array_[3].position.y);
					fixed_delta_y = scan_pose_array_[3].position.x - scan_pose_array_[1].position.x - 
						(odom_pose_array_[3].position.x - odom_pose_array_[1].position.x);
				}

				fixed_x_.push_back(fixed_delta_x);
				fixed_y_.push_back(fixed_delta_y);
				RCLCPP_WARN(this->get_logger(), "fixed_delta_x %f fixed_delta_y %f", fixed_delta_x, fixed_delta_y);

				// Update calibration flags based on results
				if(std::abs(fixed_delta_x) < delta_threshold_ && x_calib_success_flag_1) {
					x_calib_success_flag_ = true;
				}
				if(std::abs(fixed_delta_y) < delta_threshold_ && y_calib_success_flag_1) {
					y_calib_success_flag_ = true;
				}

				// Determine calibration status and set percentage
				if(x_calib_success_flag_ && !y_calib_success_flag_) {
					percent_ = 75;
					RCLCPP_INFO(this->get_logger(), "x calibration success !");
				} else if(!x_calib_success_flag_ && y_calib_success_flag_) {
					percent_ = 75;
					RCLCPP_INFO(this->get_logger(), "y calibration success !");
				} else if(x_calib_success_flag_ && y_calib_success_flag_) {
					x_y_calib_success_flag_ = true;
					RCLCPP_INFO(this->get_logger(), "x and y calibration success !");
					percent_ = 100;
					return true;
				}
			} else if(std::abs(scan_yaw - M_PI_2) <= 30*M_PI/180) {
				// Third case: angle near PI/2
				fixed_delta_y = scan_pose_array_[2].position.x - scan_pose_array_[0].position.x - 
					(odom_pose_array_[2].position.x - odom_pose_array_[0].position.x);
				fixed_delta_x = scan_pose_array_[0].position.y - scan_pose_array_[2].position.y - 
					(odom_pose_array_[0].position.y - odom_pose_array_[2].position.y);

				fixed_x_.push_back(fixed_delta_x);
				fixed_y_.push_back(fixed_delta_y);
				RCLCPP_WARN(this->get_logger(), "fixed_delta_x %f fixed_delta_y %f", fixed_delta_x, fixed_delta_y);

				// Check thresholds for third case
				if(std::abs(fixed_delta_x) < delta_threshold_) {
					x_calib_success_flag_1 = true;
				}
				if(std::abs(fixed_delta_y) < delta_threshold_) {
					y_calib_success_flag_1 = true;
				}

				// Calculate position differences based on velocity direction
				if(vel_ > 0) {
					fixed_delta_y = scan_pose_array_[3].position.y - scan_pose_array_[1].position.y - 
						(odom_pose_array_[3].position.y - odom_pose_array_[1].position.y);
					fixed_delta_x = scan_pose_array_[3].position.x - scan_pose_array_[1].position.x - 
						(odom_pose_array_[3].position.x - odom_pose_array_[1].position.x);
				} else {
					fixed_delta_y = scan_pose_array_[1].position.y - scan_pose_array_[3].position.y - 
						(odom_pose_array_[1].position.y - odom_pose_array_[3].position.y);
					fixed_delta_x = scan_pose_array_[1].position.x - scan_pose_array_[3].position.x - 
						(odom_pose_array_[1].position.x - odom_pose_array_[3].position.x);
				}

				fixed_x_.push_back(fixed_delta_x);
				fixed_y_.push_back(fixed_delta_y);
				RCLCPP_WARN(this->get_logger(), "fixed_delta_x %f fixed_delta_y %f", fixed_delta_x, fixed_delta_y);

				// Update calibration flags based on results
				if(std::abs(fixed_delta_x) < delta_threshold_ && x_calib_success_flag_1) {
					x_calib_success_flag_ = true;
				}
				if(std::abs(fixed_delta_y) < delta_threshold_ && y_calib_success_flag_1) {
					y_calib_success_flag_ = true;
				}

				// Determine calibration status and set percentage
				if(x_calib_success_flag_ && !y_calib_success_flag_) {
					percent_ = 75;
					RCLCPP_INFO(this->get_logger(), "x calibration success !");
				} else if(!x_calib_success_flag_ && y_calib_success_flag_) {
					percent_ = 75;
					RCLCPP_INFO(this->get_logger(), "y calibration success !");
				} else if(x_calib_success_flag_ && y_calib_success_flag_) {
					x_y_calib_success_flag_ = true;
					RCLCPP_INFO(this->get_logger(), "x and y calibration success !");
					percent_ = 100;
					return true;
				}
			} else if(std::abs(scan_yaw + M_PI_2) <= 30*M_PI/180) {
				// Fourth case: angle near -PI/2
				fixed_delta_y = scan_pose_array_[0].position.x - scan_pose_array_[2].position.x - 
					(odom_pose_array_[0].position.x - odom_pose_array_[2].position.x);
				fixed_delta_x = scan_pose_array_[2].position.y - scan_pose_array_[0].position.y - 
					(odom_pose_array_[2].position.y - odom_pose_array_[0].position.y);

				fixed_x_.push_back(fixed_delta_x);
				fixed_y_.push_back(fixed_delta_y);
				RCLCPP_WARN(this->get_logger(), "fixed_delta_x %f fixed_delta_y %f", fixed_delta_x, fixed_delta_y);

				// Check thresholds for fourth case
				if(std::abs(fixed_delta_x) < delta_threshold_) {
					x_calib_success_flag_1 = true;
				}
				if(std::abs(fixed_delta_y) < delta_threshold_) {
					y_calib_success_flag_1 = true;
				}

				// Calculate position differences based on velocity direction
				if(vel_ > 0) {
					fixed_delta_y = scan_pose_array_[1].position.y - scan_pose_array_[3].position.y - 
						std::abs(odom_pose_array_[1].position.y - odom_pose_array_[3].position.y);
					fixed_delta_x = scan_pose_array_[1].position.x - scan_pose_array_[3].position.x - 
						std::abs(odom_pose_array_[1].position.x - odom_pose_array_[3].position.x);
				} else {
					fixed_delta_y = scan_pose_array_[3].position.y - scan_pose_array_[1].position.y - 
						(odom_pose_array_[3].position.y - odom_pose_array_[1].position.y);
					fixed_delta_x = scan_pose_array_[3].position.x - scan_pose_array_[1].position.x - 
						(odom_pose_array_[3].position.x - odom_pose_array_[1].position.x);
				}

				fixed_x_.push_back(fixed_delta_x);
				fixed_y_.push_back(fixed_delta_y);
				RCLCPP_WARN(this->get_logger(), "fixed_delta_x %f fixed_delta_y %f", fixed_delta_x, fixed_delta_y);

				// Update calibration flags based on results
				if(std::abs(fixed_delta_x) < delta_threshold_ && x_calib_success_flag_1) {
					x_calib_success_flag_ = true;
				}
				if(std::abs(fixed_delta_y) < delta_threshold_ && y_calib_success_flag_1) {
					y_calib_success_flag_ = true;
				}

				// Determine calibration status and set percentage
				if(x_calib_success_flag_ && !y_calib_success_flag_) {
					percent_ = 75;
					RCLCPP_INFO(this->get_logger(), "x calibration success !");
				} else if(!x_calib_success_flag_ && y_calib_success_flag_) {
					percent_ = 75;
					RCLCPP_INFO(this->get_logger(), "y calibration success !");
				} else if(x_calib_success_flag_ && y_calib_success_flag_) {
					x_y_calib_success_flag_ = true;
					RCLCPP_INFO(this->get_logger(), "x and y calibration success !");
					percent_ = 100;
					return true;
				}
			}

			return false;
		}

		bool initPointsData() {
			map_stream_reader_ = std::make_shared<MapStreamReader>();
			if (!map_stream_reader_->load_map_xmlfile()) {
				RCLCPP_ERROR(this->get_logger(), "Failed to load map data");
				return false;
			}
			return true;
		}

		void OdomSubscribeCallback(const agv_msgs::msg::OdometryLite::SharedPtr msg) {
			current_odom_pose_ = msg->pose;
		}

		void PublishIgnoreFlag() {
			auto flag = std::make_shared<agv_msgs::msg::NavCorrectFlag>();

			if(!yaw_correction_forced_ && ignore_y_flag_) {
				flag->ignore_y = true;
				flag->ignore_angle = false;
			}
			else {
				flag->ignore_y = false;
				flag->ignore_angle = false;
			}

			ignore_flag_pub_->publish(*flag);
		}

		// Member variables
		double pose_x_, pose_y_, pose_theta_;
		double pose_x_old_, pose_y_old_, pose_theta_old_;
		double vel_;
		double diff_, diff_threshold_ = 1.0;
		bool isInversion_, isInversion_2_;
		bool lidar1_is_enable_, lidar2_is_enable_;
		bool record_trigger_ = false;
		double rotating_angle_;
		bool calib_success_flag_ = false;
		bool yaw_calib_success_flag_ = false;
		bool x_y_calib_success_flag_ = false;
		bool x_calib_success_flag_ = false;
		bool y_calib_success_flag_ = false;
		int trigger_ = 0;
		int node0_, node1_, node2_, node3_, node4_, node5_;
		unsigned int from_node_, to_node_;
		int percent_ = -1;
		int curr_nav_type_ = 0;
		int agv_mode_old_ = 0, agv_mode_ = 0;
		int landmark_tag_ = 0;
		bool move_dir_ = false;
		bool node1_0_called_ = false, node2_0_called_ = false, node3_0_called_ = false;
		bool node4_0_called_ = false, node5_0_called_ = false, node5_1_called_ = false;
		bool node4_1_called_ = false, node3_1_called_ = false, node2_1_called_ = false;
		bool node1_1_called_ = false;
		int move_cycle_ = 20;
		double cur_theta_ = 0;
		double landmark_theta_ = 0;
		double delta_threshold_ = 0.035;
		double pose_x_sum_ = 0, pose_y_sum_ = 0, pose_theta_sum_ = 0;
		double E_pose_x_1_, E_pose_y_1_, E_pose_theta_1_;
		double Var_pose_x_1_, Var_pose_y_1_, Var_pose_theta_1_;
		double E_pose_x_2_, E_pose_y_2_, E_pose_theta_2_;
		double Var_pose_x_2_, Var_pose_y_2_, Var_pose_theta_2_;
		double bias_x_max_1_ = 0, bias_y_max_1_ = 0, bias_theta_max_1_ = 0;
		double bias_x_max_2_ = 0, bias_y_max_2_ = 0, bias_theta_max_2_ = 0;
		double v_ = 0.08;
		double a_t_ = M_PI/1440;
		double y_bias_ = 0.15;
		int cycle_count_ = 0;
		double delta_x_ = 0, delta_y_ = 0, delta_yaw_ = 0, delta_yaw_avg_ = 0, pose_theta_avg_ = 0;
		double x2_ = 0, y2_ = 0, theta2_ = 0;
		double deltaX_, deltaY_, deltaT_;
		bool yaw_correction_forced_ = false;
		bool ignore_y_flag_ = false;
		bool calib_x_y_ = true;
		bool force_correct_ = false;
		int64_t order_num_ = 0;
		std::string lidar_msg_;
		std::vector<std::string> topics_;
		std::vector<double> fixed_x_, fixed_y_, delta_yaw_list_, pose_theta_list_;
		std::vector<float> x_vec_, y_vec_, theta_vec_;
		std::vector<bool> isInversion_vec_, isEnable_vec_;

		// ROS2 components
		rclcpp::Subscription<agv_msgs::msg::PoseWithConfidence>::SharedPtr pose_sub_;
		rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr landmark_sub_;
		rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr move_info_sub_;
		rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr mode_sub_;
		rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odometry_sub_;
		rclcpp::Publisher<agv_msgs::msg::LidarCalibPercent>::SharedPtr percent_pub_;
		rclcpp::Publisher<agv_msgs::msg::NavCorrectFlag>::SharedPtr ignore_flag_pub_;
		rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr vel_pub_;
		rclcpp::Service<agv_srvs::srv::LidarCalib>::SharedPtr lidar_calib_service_;
		rclcpp::Client<agv_srvs::srv::LocalOrder>::SharedPtr order_client_;
		rclcpp::Client<agv_srvs::srv::SetLaserCalib>::SharedPtr laser_tf_client_;
		rclcpp::Client<agv_srvs::srv::SetOperationMode>::SharedPtr mode_client_;
		rclcpp::TimerBase::SharedPtr init_map_timer_;
		rclcpp::TimerBase::SharedPtr publish_percent_timer_;
		rclcpp::TimerBase::SharedPtr record_pose_timer_;
		rclcpp::TimerBase::SharedPtr publish_ignore_flag_timer_;

		// Other components
		std::shared_ptr<MapStreamReader> map_stream_reader_;
		std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
		std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
		geometry_msgs::msg::Pose node0_pose_;
		geometry_msgs::msg::Pose cur_pose_;
		geometry_msgs::msg::Pose node2_pose_record_1_, node2_pose_record_2_;
		geometry_msgs::msg::Pose node3_pose_record_1_, node3_pose_record_2_;
		geometry_msgs::msg::Pose node4_pose_record_1_, node4_pose_record_2_;
		geometry_msgs::msg::TwistStamped current_vel_;
		geometry_msgs::msg::Pose current_odom_pose_;
		geometry_msgs::msg::Pose start_pose_;
		geometry_msgs::msg::Pose landmark_pose_;
		geometry_msgs::msg::TwistStamped vel_msg_;
		geometry_msgs::msg::Pose odom_pose_array_[4];
		geometry_msgs::msg::Pose scan_pose_array_[4];
		std::vector<PoseRecord> pose_list_1_, pose_list_2_;
		std::vector<float> bias_x_list_1_, bias_y_list_1_, bias_th_list_1_;
		std::vector<float> bias_x_list_2_, bias_y_list_2_, bias_th_list_2_;
		std::shared_ptr<boost::thread> work_thread_;
		Eigen::Vector3d point_center_;
		Eigen::Vector3d point_rotate_;
};

int main(int argc, char** argv) {
	rclcpp::init(argc, argv);
	auto node = std::make_shared<AutoCalib>();
	rclcpp::executors::MultiThreadedExecutor executor;
	executor.add_node(node);
	executor.spin();
	rclcpp::shutdown();
	return 0;
}
